#include "SpectralContrastFeat.h"

int fcmp(const void *a, const void *b)// for qsort
{
	return *(float *)a > *(float *)b ? 1: -1;
}

SpectralContrastFeat::SpectralContrastFeat(int lengthArg,int nSampRate)
{
	int i;
	length = lengthArg;
	m_nSampRate = nSampRate;
	rawSpectralContrast = new float[DIMENSION_OF_RSC];
	
	// We only calculate this slow math once
	hammingWin = new float[length];
	float hammingCoeff = (float) 6.2831852 / (length - 1);
	for (i=0; i<length; i++) {
		hammingWin[i] = (float) (0.54 - 0.46 * cos(hammingCoeff * i));
	}

	
	// compute necessary FFT number
	FFT_Point = 1;
	FFT_Order = 0;
	int minLength4FFT = (int) ((1 + ZERO_PADDING_RATE) * length);
	while (FFT_Point < minLength4FFT)
	{
		FFT_Point = FFT_Point << 1;
		FFT_Order ++;
	}
	fft = new SplitRadixFFT(FFT_Order);

	// compute the FFT index at the beginning of each subband and corresponding width
	boundaryTable = new int[SUBBAND_NUM];
	subbandSize = new int[SUBBAND_NUM];
	float boundaryFreqTable[SUBBAND_NUM] = {0, 200, 400, 800, 1600, 3200};

	for (i=0; i<SUBBAND_NUM; i++)
	{
		boundaryTable[i] = (int) ceil(boundaryFreqTable[i] / m_nSampRate * FFT_Point);
	}
	
	for (i=0; i<SUBBAND_NUM-1; i++)
	{
		subbandSize[i] = boundaryTable[i+1] - boundaryTable[i];
	}
	subbandSize[SUBBAND_NUM-1] = FFT_Point/2 - boundaryTable[SUBBAND_NUM-1];

}

//  [11/26/2010 Song]
SpectralContrastFeat::SpectralContrastFeat(int nWinsize, int framePeriod, int nSampRate)
{
	int i;
//	length = lengthArg;
	m_nWinSize = nWinsize*nSampRate/1000;
	m_nFrameSize = framePeriod*nSampRate/1000;
	m_nSampRate = nSampRate;
	rawSpectralContrast = new float[DIMENSION_OF_RSC];
	
	// We only calculate this slow math once
	hammingWin = new float[m_nWinSize];
	float hammingCoeff = (float) 6.2831852 / (m_nWinSize - 1);
	for (i=0; i<m_nWinSize; i++) {
		hammingWin[i] = (float) (0.54 - 0.46 * cos(hammingCoeff * i));
	}
	
	
	// compute necessary FFT number
	FFT_Point = 1;
	FFT_Order = 0;
	
	while (FFT_Point < m_nWinSize)
	{
		FFT_Point = FFT_Point << 1;
		FFT_Order ++;
	}
	fft = new SplitRadixFFT(FFT_Order);
	
	// compute the FFT index at the beginning of each subband and corresponding width
	boundaryTable = new int [SUBBAND_NUM];
	subbandSize = new int [SUBBAND_NUM];
	float boundaryFreqTable[SUBBAND_NUM] = {0, 200, 400, 800, 1600, 3200};
	
	for (i=0; i<SUBBAND_NUM; i++)
	{
		boundaryTable[i] = (int) ceil(boundaryFreqTable[i] / m_nSampRate * FFT_Point);
	}
	
	for (i=0; i<SUBBAND_NUM-1; i++)
	{
		subbandSize[i] = boundaryTable[i+1] - boundaryTable[i];
	}
	subbandSize[SUBBAND_NUM-1] = FFT_Point/2 - boundaryTable[SUBBAND_NUM-1];
	
}

SpectralContrastFeat::~SpectralContrastFeat()
{
	if (rawSpectralContrast)	delete []rawSpectralContrast;
	if (hammingWin)				delete []hammingWin;
	if (boundaryTable)			delete []boundaryTable;
	if (subbandSize)			delete []subbandSize;
	if (fft)					delete fft;
}

int SpectralContrastFeat::RawSpectralContrast(short *buffer)
{

	// check if the input arguments are legal
	if (length < 0 || buffer == NULL || (buffer+length-1) == NULL) {
		printf("Error: illegal arguments.");
		return -1;
	}
	
	int i, j;
	
	if (FFT_Point < length)
	{
		printf("Error: FFT_Point is larger than frame length.");
		return -1;
	}
	
	// We don't do 'in-place' FFT so we copy it into a new array first
	// In the meantime, we multiply a hamming window
	
	float* X = new float[FFT_Point];
	for (i=0; i<length; i++) 
	{
		X[i] = (float) buffer[i] * hammingWin[i];
	}
	
	for (i=length; i<FFT_Point; i++) 
	{
		X[i] = 0;
	}
	
	fft->XForm(X);

	// amplitude spectrum
	X[0] = (float) sqrt(X[0]*X[0]);
	for (i=1; i<FFT_Point/2; i++)
	{
		X[i] = (float) sqrt(X[i]*X[i]+X[FFT_Point-i]*X[FFT_Point-i]);
	}

	// pointer to each subband
	float **subband_pointer = new float* [SUBBAND_NUM];
	for (i=0; i<SUBBAND_NUM; i++)
	{
		subband_pointer[i] = &X[boundaryTable[i]];

		// sort each subband in ascending order
		qsort(subband_pointer[i], subbandSize[i], sizeof(float), fcmp);

		// compute peak and valley
		float numFFT_Considered = subbandSize[i]*NEIGHBORHOOD_ALPHA;
		float peak = 0;
		for (j=0; j<numFFT_Considered; j++)
		{
			peak += *(subband_pointer[i]+subbandSize[i]-1-j);
			//printf("For subband [%u] Peak add [%f]\n", i, *(subband_pointer[i]+subbandSize[i]-1-j));
		}
		float valley = 0;

		for (j=0; j<numFFT_Considered; j++)
		{
			valley += *(subband_pointer[i]+j);
			//printf("For subband [%u] Valley add [%f]\n", i, *(subband_pointer[i]+j));
		}
		rawSpectralContrast[2*i] = (float) log(peak/numFFT_Considered);
		rawSpectralContrast[2*i+1] = (float) log(valley/numFFT_Considered);
		rawSpectralContrast[2*i] = rawSpectralContrast[2*i] - rawSpectralContrast[2*i+1];
	}

	delete []subband_pointer;
	delete []X;
	return 0;

}

//  [11/26/2010 Song]
int SpectralContrastFeat::SpectralContrast(short *waveData, int NumSamples, int &nFramNum)
{
	
	// check if the input arguments are legal
	if (NumSamples < 0 || waveData == NULL) {
		printf("Error: illegal arguments.");
		return -1;
	}
	
	int i, j;
	nFramNum = (NumSamples-m_nWinSize)/m_nFrameSize+1;
	pf_FeatBuf = new float[nFramNum*FEAT_DIM];
	memset(pf_FeatBuf, 0, sizeof(float)*nFramNum*FEAT_DIM);
	
	// We don't do 'in-place' FFT so we copy it into a new array first
	// In the meantime, we multiply a hamming window
	short *pCurWave = waveData;
	float *pCurFeat = pf_FeatBuf;
	float *X = new float[FFT_Point];
	float **subband_pointer = new float* [SUBBAND_NUM];
	for (int nFrmIdx=0; nFrmIdx<nFramNum; nFrmIdx++)
	{
		
		for (i=0; i<m_nWinSize; i++) 
		{
			X[i] = (float) pCurWave[i] * hammingWin[i];
		}
		
		for (i=m_nWinSize; i<FFT_Point; i++) 
		{
			X[i] = 0;
		}
		
		fft->XForm(X);
		
		// amplitude spectrum
		X[0] = (float) sqrt(X[0]*X[0]);
		for (i=1; i<FFT_Point/2; i++)
		{
			X[i] = (float) sqrt(X[i]*X[i]+X[FFT_Point-i]*X[FFT_Point-i]);
		}
		
		// pointer to each subband
//		
		for (i=0; i<SUBBAND_NUM; i++)
		{
			subband_pointer[i] = &X[boundaryTable[i]];
			
			// sort each subband in ascending order
			qsort(subband_pointer[i], subbandSize[i], sizeof(float), fcmp);
			
			// compute peak and valley
			float numFFT_Considered = subbandSize[i]*NEIGHBORHOOD_ALPHA;
			float peak = 0;
			for (j=0; j<numFFT_Considered; j++)
			{
				peak += *(subband_pointer[i]+subbandSize[i]-1-j);
				//printf("For subband [%u] Peak add [%f]\n", i, *(subband_pointer[i]+subbandSize[i]-1-j));
			}

			float valley = 0;			
			for (j=0; j<numFFT_Considered; j++)
			{
				valley += *(subband_pointer[i]+j);
				//printf("For subband [%u] Valley add [%f]\n", i, *(subband_pointer[i]+j));
			}
			rawSpectralContrast[2*i] = (float) log(peak/numFFT_Considered+1);
			rawSpectralContrast[2*i+1] = (float) log(valley/numFFT_Considered+1);
			rawSpectralContrast[2*i] = rawSpectralContrast[2*i] - rawSpectralContrast[2*i+1];
		}
		
		//DCT
		for (i=0; i<FEAT_DIM/2; i++)
		{
			float x = (float)((i+1)*PI/SUBBAND_NUM);

			for (j=0; j<SUBBAND_NUM; j++)
			{
				float y = cos(x*(j+0.5));
				pCurFeat[i] += rawSpectralContrast[2*j] * y;
				pCurFeat[i+FEAT_DIM/2] += rawSpectralContrast[2*j+1] * y;
			}
		}

		pCurWave += m_nFrameSize;
		pCurFeat += FEAT_DIM;
		

	}
	
	DoNorm(pf_FeatBuf, nFramNum, FEAT_DIM);


	delete []subband_pointer;
	delete []X;
	return 0;
	
}

void SpectralContrastFeat::DoNorm(float *data, int n, int vSize)
{
	double sum;
	float *fp,fmean;
	int i,j;
	
	// 2007.01.18 plu : 
	float fVar,fsumVar;
	
	for (i=0; i<vSize; i++)
	{
		/* find mean over i'th component */
		sum = 0.0;
		fsumVar = 0.f;
		fp = data+i;
		for (j=0;j<n;j++){
			sum += *fp; 
			fsumVar += (*fp)*(*fp);
			fp += vSize;
		}
		fmean = (float)(sum / (double)n);
		fVar = (float)sqrt((fsumVar / (double)n) - fmean*fmean);
		
		/* normaliz variance from i'th components */
		fp = data+i;
		for (j=0;j<n;j++)
		{
			*fp = (*fp - fmean)/fVar;
			fp += vSize;
		}
	}
}